#include "ThingpleTcpServerHandler.h"


ThingpleTcpServerHandler::ThingpleTcpServerHandler(void)
    :_stop(false), _server(42000, *this, 0),_serialPort("/dev/ttyAMA0")
{
}

ThingpleTcpServerHandler::ThingpleTcpServerHandler(int port, const QString &serialPort)
    :_stop(false),_server(port, *this, 0),_serialPort(serialPort)
{

}
void ThingpleTcpServerHandler::assignPhysicalAnt(char i)
{
    char assign_ant[8] = {0x01,0x00,0x04,0x07,i,0x00,0x00,0x00};//assign physical antenna 1
    write(SerialFd,assign_ant,8);
    usleep(1000);
}
void ThingpleTcpServerHandler::startServer()
{
    _server.start();
}

void ThingpleTcpServerHandler::left(char* dst, char* src, int n)
{
    char* p = src;
    char* q = dst;
    int len = strlen(src);
    if(n>len) n = len;
    while(n--)
    {
       *(q++)=*(p++);
    }
    *q='\0';
}

void ThingpleTcpServerHandler::start(void)
{
    std::cout << "Start handler !" << std::endl;
}

void ThingpleTcpServerHandler::stop(void)
{
    std::cout << "Stop handler !" << std::endl;
}

int ThingpleTcpServerHandler::charFlow(char ant)
{
    unsigned char Info[2048];
    char channel, antenna, fast, type, length, padding, epcLength ='\0';
    unsigned char num = 0;
    short RSSI = 0;
    float rssi = 0;
    int  len, flag, tid_start, sendNum, readSerialNum= 0;
    int shift = 16;     // jump over the fisrt 16 bytes which are command begin packet;
    //  char (*SendTagInfo)[32] = new char[38][32];
    char SendInfo[31];
    char TagPrint[32];
    int fd = getSocket();
    // adjust the antenna order to 1,2,3,4
    if(ant == 0)
    {
        ant = 1;
    }else if(ant == 1)
    {
        ant = 4;
    }

    write(SerialFd,start_Inv_cde,8);
    usleep(300000);

    readSerialNum = read(SerialFd,Info,2048);
    printf("Read Serial Number : %d\n",readSerialNum);

    if(readSerialNum >= 32) // begin-end pair is 32 bytes
    {
        do
        {
            fast = Info[1+shift]&(0x03<<2); // fast = 0 means model is TagFocus, fast = 4 means model is FastID;
    //        qDebug()<< "fast = "<< (unsigned char)fast;
            padding = Info[1+shift]&(0x03<<6); // padding bytes to force 32-bit packet boundary
    //        qDebug()<< "padding = "<< (unsigned short)padding;
            type = Info[2+shift];
    //        qDebug()<< "packet type : "<< (unsigned char)type;

            if(type == 5)
            {
                length = (Info[4+shift] & 0xFF) + ((Info[5+shift] & 0xFF)<<8);
                len = length*4+8;
                SendInfo[1] = Info[16+shift];
                SendInfo[2] = Info[17+shift];
                antenna = Info[19+shift];
                unsigned char ante = antenna & 0xC0;
                switch(ante)
                {
                    case 0x00:
                        channel = 1;
                        break;
                    case 0x40:
                        channel = 4;
                        break;
                    case 0x80:
                        channel = 2;
                        break;
                    case 0xC0:
                        channel = 3;
                        break;
                }

                SendInfo[0] = channel;
                RSSI =  (SendInfo[1] & 0xFF) + ((SendInfo[2] & 0xFF)<<8);
                rssi = (float)RSSI/10;
                qDebug() << "RSSI :" << rssi;
                if(fast == 0)  // not including TID
                {
                    epcLength = len - 24 -padding;
                    for(int i=0;i<epcLength;i++)
                    {
                        SendInfo[3+i]=Info[22+shift+i];
                    }
                    flag = 3 + epcLength;

                }else if(fast == 4) //including TID
                {
                    epcLength = len - 24 -12 - padding;
                    for(int i=0;i<epcLength;i++)
                    {
                        SendInfo[3+i]=Info[22+shift+i];
                    }

                    tid_start = 22+epcLength+1;

                    for(int i=0;i<12;i++)
                    {
                        SendInfo[3+epcLength+i]=Info[tid_start+shift+i];
                    }
                    flag = 3 + epcLength + 12 ;

                }

                putInt(flag);
                qDebug() << "Number :" << flag;

                for(int j=0;j<flag;j++)
                {
                    sprintf(TagPrint+2*j,"%02X",SendInfo[j]);
                }
                TagPrint[2*flag]=0;
                qDebug() << "SendInfo:"<< TagPrint;

                sendNum = send(fd,SendInfo,flag,0);
                if(sendNum != flag)
                {
                    qDebug()<<"send number :"<< sendNum;
                }

                shift +=len;
                len = 0;
                num = num + 1;
                fast = '\0';
                padding = '\0';
                type = '\0';

            }else if((type != 5) && (type != 1))
            {
                putInt(2); // error code 2 means packet type is not supposed to be here
                return 2;
            }
        }while(type != 1);

        qDebug()<< "reading " << (unsigned short)num << "Tags from ant" << (unsigned short) ant;

    }else
    {
         putInt(3); //error code 3 means received number of bytes is not enough,checking the module is needed.
         return 3;
    }
    // memset(Info,0,sizeof(Info));
    // init the Info[2048]//
    for(int j=0;j<2048;j++)
    {
        Info[j]=0;
    }
    return 0;
}

void ThingpleTcpServerHandler::onConnect(int fd)
{
    std::cout << "Receive connection on socket !" << std::endl;
    setSocket(fd);
    setTimeout(0);
    int flags = fcntl(fd,F_GETFL,0);
    fcntl(fd,F_SETFL,flags&~O_NONBLOCK);

    /**
     * Opening serial line.
     */
    AlienGenSerialLine ttl(this->_serialPort.toStdString());
    SerialFd=ttl.open();
    if(SerialFd< 0)
    {
        qDebug() << "Serial line not available!" << this->_serialPort;
        closesocket(fd);
        _stop = true;
        return;
    }

    unsigned int size = 0;
    char receive[8] = {0};

    write(SerialFd,is_active,8);
    usleep(2500);
    read(SerialFd,receive,8);
    if(receive[4]==0)
    {
        qDebug() << "MAC is active !";
        putInt(0);

    }
    else
    {
        qDebug() << "MAC is not active !";
        putInt(1); // error code 1 means Mac is not active;
         return;
    }

    int ret1, ret2 = 0;
    write(SerialFd,cancel_cde,8);
    usleep(2500);

    ret1 = read(SerialFd,receive,8);

    ret2 = memcmp(receive,cancel_response,8);
    if(ret2!=0)
    {
        qDebug()<< "cancel_code's responding is not right !";
    }else
    {
        qDebug()<< "cancel_code's responding is right !";
    }

    char ReceiveCommand[12]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};


    do{
        size = recv(fd, ReceiveCommand, 11, 0);

        if(size != 11)
        {
            qDebug() << "received command size = " << size;
            return;
        }
        //  data=getString(&size); segmentation fault
      //  strcpy(Info,data);     segmentation fault
       // qDebug() << "strcpy";
      //  left(des,Info,5);
      //  qDebug() << "left";

        qDebug()<< "Receive :" << ReceiveCommand ;
        if(strncmp(ReceiveCommand,"INITI",5)==0)
        {
            char ant = ReceiveCommand[5];
            char model = ReceiveCommand[10];

            write(SerialFd,select_logical_ant,8);
            usleep(1000);

            write(SerialFd,enable_logical_ant,8);
            usleep(1000);

            if(ant == 1)
            {
                char assign_ant[8] = {0x01,0x00,0x04,0x07,0x00,0x00,0x00,0x00};//assign physical antenna
                write(SerialFd,assign_ant,8);
                usleep(1000);
                qDebug()<< "select ant:" << ant ;
            }else if((ant == 2) ||(ant == 3))
            {
                char assign_ant[8] = {0x01,0x00,0x04,0x07,ant,0x00,0x00,0x00};//assign physical antenna
                write(SerialFd,assign_ant,8);
                usleep(1000);
                qDebug()<< "select ant:" << ant ;
            }else if(ant == 4)
            {
                char assign_ant[8] = {0x01,0x00,0x04,0x07,0x01,0x00,0x00,0x00};//assign physical antenna
                write(SerialFd,assign_ant,8);
                usleep(1000);
                qDebug()<< "select ant:" << ant ;
            }else
            {
                write(SerialFd,assign_physical_ant0,8);
                usleep(1000);
                qDebug() << "select ant 0 and wait for polling !";
            }

            char power[8] = {0x01,0x00,0x06,0x07,ReceiveCommand[6],ReceiveCommand[7],0x00,0x00};//power
            //char power[8] = {0x01,0x00,0x06,0x07,0x2C,0x01,0x00,0x00};
            write(SerialFd,power,8);
            usleep(1000);

            char dwell_time[8] = {0x01,0x00,0x05,0x07,ReceiveCommand[8],ReceiveCommand[9],0x00,0x00};
            //char dwell_time[8] = {0x01,0x00,0x05,0x07,0xC8,0x00,0x00,0x00};//dwell time
            write(SerialFd,dwell_time,8);
            usleep(1000);

            char cycle_times[8] = {0x01,0x00,0x00,0x07,0x00,0x00,0x00,0x00};//cycle times
            write(SerialFd,cycle_times,8);
            usleep(1000);

            if(model == 1)
            {
                write(SerialFd,Impinj_Extensions_TagFocus,8);
                usleep(1000);

                write(SerialFd,Query_CFG_cde,8);
                usleep(1000);

                write(SerialFd,INV_PARM_2_CDE,8);
                usleep(1000);

            }else if(model == 2)
            {
                write(SerialFd,Impinj_Extensions_FastID,8);
                usleep(1000);

                write(SerialFd,Query_CFG_cde,8);
                usleep(1000);

                write(SerialFd,INV_PARM_2_CDE,8);
                usleep(1000);
            }
          //  sendBuffer("INITI",&p);
            putInt(0);

        }else if(strncmp(ReceiveCommand,"ANTEN",5)==0)
        {
            char ant = ReceiveCommand[5];

            write(SerialFd,select_logical_ant,8);
            usleep(1000);

            write(SerialFd,enable_logical_ant,8);
            usleep(1000);

            if(ant == 1)
            {
                char assign_ant[8] = {0x01,0x00,0x04,0x07,0x00,0x00,0x00,0x00};//assign physical antenna 1
                write(SerialFd,assign_ant,8);
                usleep(1000);
                qDebug()<< "select ant:" << ant ;
            }else if((ant == 2) ||(ant == 3))
            {
                assignPhysicalAnt(ant);
                qDebug()<< "select ant:" << ant ;
            }else if(ant == 4)
            {
                char assign_ant[8] = {0x01,0x00,0x04,0x07,0x01,0x00,0x00,0x00};//assign physical antenna 4
                write(SerialFd,assign_ant,8);
                usleep(1000);
                qDebug()<< "select ant:" << ant ;
            }else
            {
                write(SerialFd,assign_physical_ant0,8);
                usleep(1000);
                qDebug() << "select ant 0 and wait for polling !";
            }
            //sendBuffer("ANTEN",&p);
            putInt(0);
        }else if(strncmp(ReceiveCommand,"POWER",5)==0)
        {
            char power[8] = {0x01,0x00,0x06,0x07,ReceiveCommand[6],ReceiveCommand[7],0x00,0x00};//power
            write(SerialFd,power,8);
            usleep(1000);                 
            putInt(0);
        }else if(strncmp(ReceiveCommand,"DWELL",5)==0)
        {
            char dwell_time[8] = {0x01,0x00,0x05,0x07,ReceiveCommand[8],ReceiveCommand[9],0x00,0x00};//dwell time
            write(SerialFd,dwell_time,8);
            usleep(1000);
            putInt(0);
        }else if(strncmp(ReceiveCommand,"MODEL",5)==0)
        {
            char model = ReceiveCommand[10];
            if(model == 1)
            {
                write(SerialFd,Impinj_Extensions_TagFocus,8);
                usleep(1000);

                write(SerialFd,Query_CFG_cde,8);
                usleep(1000);

                write(SerialFd,INV_PARM_2_CDE,8);
                usleep(1000);

            }else if(model == 2)
            {
                write(SerialFd,Impinj_Extensions_FastID,8);
                usleep(1000);

                write(SerialFd,Query_CFG_cde,8);
                usleep(1000);

                write(SerialFd,INV_PARM_2_CDE,8);
                usleep(1000);
            }
         //   sendBuffer("MODEL",&p);
            putInt(0);
        }else if(strncmp(ReceiveCommand,"ACTIV",5)==0)
        {
            write(SerialFd,is_active,8);
            usleep(2500);
            read(SerialFd,receive,8);

            if(receive[4]==0)
            {
                qDebug() << "MAC is active !";
                //sendBuffer("ACTIV",&p);
                putInt(0);
            }
            else
            {
                qDebug() << "MAC is not active !";
                //sendBuffer("NOTAC",&p);
                putInt(1);
                return;
            }

        }else if(strncmp(ReceiveCommand,"START",5)==0)
        {
           char ant = ReceiveCommand[5];
           int ret = 0;

           if(ant < 5 && ant > 0)
           {
               if(ant == 1)
               {
                   ant = 0;
                   assignPhysicalAnt(ant);

               }else if(ant == 4)
               {
                   ant = 1;
                   assignPhysicalAnt(ant);
               }else
               {
                   assignPhysicalAnt(ant);
               }

               ret = charFlow(ant);
               if(ret == 2)
               {
                   qDebug() << "error code 2: packet type error !";
               }else if(ret == 3)
               {
                   qDebug() << "error code 3: Bytes return from Arm7 are not enough for a Begin-End pair !";
               }


            }else if(ant == 5)
           {
               for(char i=0;i<2;i++)
               {
                   assignPhysicalAnt(i);
                   ret = charFlow(i);
                   if(ret == 2)
                   {
                       qDebug() << "error code 2: packet type error !";
                   }else if(ret == 3)
                   {
                       qDebug() << "error code 3: Bytes return from Arm7 are not enough for a Begin-End pair !";
                   }
               }
           }else if(ant == 6)
           {
               for(char i=0;i<4;i++)
               {
                   assignPhysicalAnt(i);
                   ret = charFlow(i);
                   if(ret == 2)
                   {
                       qDebug() << "error code 2: packet type error !";
                   }else if(ret == 3)
                   {
                       qDebug() << "error code 3: Bytes return from Arm7 are not enough for a Begin-End pair !";
                   }
               }
           }
            //sendBuffer("START",&p);
        }
    }while(size != -1);//(data!=NULL);

    closesocket(fd);
    _stop = true;
}
